# 我实在没办法了，出此下策 help！！！！！
# windows 能收能发 ubuntu 只能收windows 的
# 开两个 socket ，一个 接收  一个 转发(转发到 224.0.2.2，42102)
# 通过路由器后即使在一个网段也收不到 ARS548 的数据
# 不设置网关会有问题 nodevice
# 有python环境就可以运行

# encode utf-8
# TODO 汽研所需要做的
# TODO 1. 代码同步
# TODO 2. 代码方案测试
# TODO 3. 延时测试
# TODO 4. 户外数据 可视化 及视频
# TODO 4. ROS bag 记录
# TODO 5. 回放

import socket
import struct
import sys
import logging
import time
from datetime import datetime
if __name__=="__main__":
    print("##########################")
    print("TRICK: 转发 ARS548 数据")
    print("#########warning##########")
    print("1. 不设置网关有几率 no device")
    print("2. 需要手动设置windows的IP\n\teg:\n\tip:192.168.1.100 \n\tmask:255.255.255.0\n\tgateway:192.168.1.1 ")
    print("3. 决明子 或 其他 虚拟 网卡设备有影响")
    print("##########################")

    multicast_group = '224.0.2.2'
    server_address = ('',42102)

    # Create the socket
    sock_recv = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)

    # Bind to the server address
    sock_recv.bind(server_address)
    ttl = struct.pack('b', 2)
    sock_recv.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_TTL, ttl)
    sock_recv.settimeout(1)


    # Tell the operating system to add the socket to the multicast group
    # on all interfaces.
    group = socket.inet_aton(multicast_group)
    mreq = struct.pack('4sL', group, socket.INADDR_ANY)
    sock_recv.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, mreq)

    mgrp = "224.0.2.2"
    mport = 42103
    sock_send = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP)
    sock_send.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)

    
    last_normal_time = 0
    last_timed_out_time = 0
    while True:
        try:
            
            # Receive/respond loop
            data, address = sock_recv.recvfrom(50000)
            # MESSAGE = b'\x00\00\x01\x53\x00\x00\x00\x2a\x00\x00\xf1\x20\x01\x01\x02\x00\x37\xb6\x25\x60\x00\x00\x21\x8d\xa8\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00'
            
            sock_send.sendto(data, (mgrp, mport))
            if(time.time()-last_normal_time > 10):
                last_normal_time = time.time()
                now = datetime.now()
                timestamp = now.strftime('%Y-%m-%d %H:%M:%S')
                print("Forwarding is all right,[Timestamp]"+timestamp)
        except Exception as e:
            if str(e) == "timed out":
                if(time.time()-last_normal_time > 10):
                    last_normal_time = time.time()
                    now = datetime.now()
                    timestamp = now.strftime('%Y-%m-%d %H:%M:%S')
                    print("无法从ARS548接收消息,请检查,[Timestamp]"+timestamp)
            else:
                print(e)
